#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # 定义launch参数变量
    bag_file = LaunchConfiguration('bag_file')
    point_cloud_topic = LaunchConfiguration('point_cloud_topic')
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    
    # 声明命令行参数
    declare_bag_file_arg = DeclareLaunchArgument(
        'bag_file',
        default_value='/home/cheng/Documents/data_718_4',
        description='Path to the ROS2 bag file'
    )
    
    declare_point_cloud_topic_arg = DeclareLaunchArgument(
        'point_cloud_topic',
        default_value='/livox/lidar_avia',
        description='Point cloud topic name'
    )
    
    # 启动ROS2 bag回放
    play_bag_cmd = ExecuteProcess(
        cmd=['ros2', 'bag', 'play', bag_file, '--clock', '-l'],
        output='screen'
    )
    
    # 启动C++版本的点云处理节点
    lidar_processor_node = Node(
        package='lidar_processing_cpp',  # 注意：这里是C++功能包的名称
        executable='lidar_processor_cpp',  # C++可执行文件名称
        name='lidar_processor',
        parameters=[
            {'use_sim_time': use_sim_time},
            {'input_cloud': point_cloud_topic}
        ],
        output='screen'
    )
    
    return LaunchDescription([
        declare_bag_file_arg,
        declare_point_cloud_topic_arg,
        play_bag_cmd,
        lidar_processor_node
    ])
    